#include "match.h"
namespace perception
{
    namespace algorithm
    {
        static inline double normalize_angle_positive(double angle)
        {
            // Normalizes the angle to be 0 to 2*M_PI.
            // It takes and returns radians.
            return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
        }
        static inline double normalize_angle(double angle)
        {
            // Normalizes the angle to be -M_PI circle to +M_PI circle
            // It takes and returns radians.
            double a = normalize_angle_positive(angle);
            if (a > M_PI)
                a -= 2.0 * M_PI;
            return a;
        }

        static inline double shortest_angular_distance(double from, double to)
        {
            // Given 2 angles, this returns the shortest angular difference.
            // The inputs and outputs are radians.
            // The result would always be -pi <= result <= pi.
            // Adding the result to "from" results to "to".
            return normalize_angle(to - from);
        }
        Match::Match(track_config &config)
        {
            this->max_confidence = config.max_confidence;
            this->dconfidence = config.dconfidence;
            this->now_v_ratio = config.now_v_ratio;
        }
        void Match::Init(unsigned long int id, LidarObject &new_points, const double &dt, const Localization &ego_pose)
        {
            // std::cout << "id:" << this->id << std::endl;
            // std::cout << "cx0:" << this->cx << " cy:" << this->cy << std::endl;
            this->id = id;
            this->age = 1;
            this->x = new_points.x;
            this->y = new_points.y;
            // this->x = this->x + dt * ego_pose.vx * 3;
            // this->y = this->y + dt * ego_pose.vy * 3;
            this->z = new_points.z;
            this->type = int(new_points.type);
            this->height_box = new_points.height;
            // std::cout << "~~~~~Init~~~~~~~~~~" << int(new_points.type) << std::endl;
            pre_obj = new_points;
            this->confidence = 3;
            LshapeTracker l_shape_tracker_ukf(new_points.corner_point.x, new_points.corner_point.y,
                                              new_points.l1, new_points.l2, new_points.thetal1, dt);
            // LshapeTracker l_shape_tracker_ukf(new_points.x, new_points.y,
            //                                   new_points.l1, new_points.l2, new_points.thetal1, dt);
            this->Lshape = l_shape_tracker_ukf;
            // Lshape.BoxModelCenter(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            // Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            // std::cout << "cx1:" << this->cx << " cy:" << this->cy << std::endl;
            this->psi = new_points.heading;
            this->length_box = new_points.length;
            this->width_box = new_points.width;
            this->cvx = 0;
            this->cvy = 0;
            this->ax = 0;
            this->ay = 0;
            CornerPoint p;
            p.x = this->x;
            p.y = this->y;
            all_points.emplace_back(p);
            pre_heading = new_points.heading;
            sum_x = sum_x + new_points.x;
            sum_y = sum_y + new_points.y;
            cp = new_points.corner_point;
            if (id > 255)
                id = 1;
        }
        void Match::update2(LidarObject &new_points, const double &dt, const Localization &ego_pose)
        {
            float old_x = this->x;
            float old_y = this->y;
            float new_x = new_points.x;
            float new_y = new_points.y;
            float old_length = this->length_box;
            float old_width = this->width_box;
            this->age++;
            this->type = int(new_points.type);
            this->height_box = new_points.height;
            pre_heading = new_points.heading;
            pre_obj = new_points;
            if (track_type_ == track_type::MATCHED)
                this->confidence = this->confidence + this->dconfidence;
            else if (track_type_ == track_type::MATCHING_FAILED)
                this->confidence = this->confidence - this->dconfidence;
            Lshape.update(new_points.thetal1, new_points.corner_point.x, new_points.corner_point.y, new_points.l1, new_points.l2, dt, new_points.cells.size());
            Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            this->x = this->cvx * dt + new_points.x;
            this->y = this->cvy * dt + new_points.y;
            new_points.corner_point.x = new_points.corner_point.x + this->cvx * dt;
            new_points.corner_point.y = new_points.corner_point.y + this->cvy * dt;
            cp = new_points.corner_point;
        }
        void Match::update(LidarObject &new_points, const double &dt, const Localization &ego_pose)
        {
            float old_x = this->x;
            float old_y = this->y;
            float new_x = new_points.x;
            float new_y = new_points.y;
            this->age++;
            this->type = int(new_points.type);
            this->height_box = new_points.height;
            pre_obj = new_points;
            if (track_type_ == track_type::MATCHED)
                this->confidence = this->confidence + this->dconfidence;
            else if (track_type_ == track_type::MATCHING_FAILED)
                this->confidence = this->confidence - this->dconfidence;
            // Lshape.update(new_points.thetal1, new_points.corner_point.x, new_points.corner_point.y, new_points.l1, new_points.l2, dt, new_points.cells.size());
            // Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            Lshape.updateCenter(new_points.thetal1, new_points.x, new_points.y, new_points.l1, new_points.l2, dt, new_points.cells.size());
            // Lshape.BoxModelCenter(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            Lshape.BoxModelCenter(x, y, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);

            this->length_box = new_points.length;
            this->width_box = new_points.width;
            this->psi = new_points.heading;
            this->x = new_points.x;
            this->y = new_points.y;
            this->z = new_points.z;
            auto vx_tmp = (new_x - old_x) / dt;
            auto vy_tmp = (new_y - old_y) / dt;
            if (std::fabs(vx_tmp - this->cvx) > 2)
            {
                if (vx_tmp > this->cvx)
                    this->cvx = this->cvx + 2;
                else
                    this->cvx = this->cvx - 2;
            }

            if (std::fabs(vy_tmp - this->cvy) > 2)
            {
                if (vy_tmp > this->cvy)
                    this->cvy = this->cvy + 2;
                else
                    this->cvy = this->cvy - 2;
            }

            if (CheckStatic())
            {
                CornerPoint p;
                p.x = this->x;
                p.y = this->y;
                all_points.emplace_back(p);
                sum_x = sum_x + this->x;
                sum_y = sum_y + this->y;
            }
            else
            {
                this->x = this->x + dt * this->cvx;
                this->y = this->y + dt * this->cvy;
                CornerPoint p;
                p.x = this->x;
                p.y = this->y;
                all_points.emplace_back(p);
                sum_x = sum_x + this->x;
                sum_y = sum_y + this->y;
            }
        }
        void Match::update(const double &dt, const Localization &ego_pose)
        {
            this->age++;
            this->confidence = this->confidence - this->dconfidence;
            Lshape.update(pre_obj.thetal1, pre_obj.corner_point.x, pre_obj.corner_point.y, pre_obj.l1, pre_obj.l2, dt, pre_obj.cells.size());
            // Lshape.updateCenter(pre_obj.thetal1, pre_obj.x, pre_obj.y, pre_obj.l1, pre_obj.l2, dt, pre_obj.cells.size());
            Lshape.BoxModel(cx, cy, cvx, cvy, th, psi, comega, L1_box, L2_box, length_box, width_box);
            this->x = this->cvx * dt + this->x;
            this->y = this->cvy * dt + this->y;
        }
        bool Match::CheckStatic()
        {
            if (all_points.size() > 3)
            {
                float detx = std::fabs(sum_x / all_points.size() - this->x);
                float dety = std::fabs(sum_y / all_points.size() - this->y);
                // ERROR("detx:{},dety:{},sum_x:{},sum_y:{},size:{}", detx, dety, sum_x, sum_y, all_points.size());
                if (detx < 2 && dety < 2)
                {
                    this->cvx = 0;
                    this->cvy = 0;
                    this->ax = 0;
                    this->ay = 0;
                    this->cx = detx;
                    this->cy = dety;
                    return true;
                }
            }

            return false;
        }
    }
}